#include <stdio.h>
#include <stdlib.h>

#include "..\cuCommon\/cuTypes.h"
#include "..\RadixSort\RadixSort.h"
#include "..\RadixSort\IceRevisitedRadix.h"
#include "..\rtCommon\Histopyramid.h"
#include "..\rtCommon\Random.h"

#include <D3DX10Math.h>

#include "RegularGrid.h"

#include "..\glew\glew.h"
#include "..\glut\glut.h"

#define _USE_MATH_DEFINES 1
#include <math.h>

matrix4x4 a, b, c;

struct KeyValuePair
{
	float x;
	float value;
};

CRadixSort<KeyValuePair> kvp_sorter;
CRadixSort< int32>       int_sorter;
CRadixSort<uint32>       uint_sorter;
CRadixSort<float>        float_sorter;
RadixSort                sorter;
f_HistoPyramid           histopyramid;
int main(int argc, char *argv[])
{
	float3 x, y, z;
	D3DXVECTOR3 dx_x, dx_y, dx_z;

	srand(103987901);

	dx_x.x = x.x = (rand() % 1025) / 1024.0f;
	dx_x.y = x.y = (rand() % 1025) / 1024.0f;
	dx_x.z = x.z = (rand() % 1025) / 1024.0f;
	dx_y.x = y.x = (rand() % 1025) / 1024.0f;
	dx_y.y = y.y = (rand() % 1025) / 1024.0f;
	dx_y.z = y.z = (rand() % 1025) / 1024.0f;

	printf("\nTest with D3D functions\n");
	/*************************************************************************/
	printf("Testing vector operation *\n");
	x *= 2.0f;
	dx_x *= 2.0f;
	printf("Operator *: \n\t(%f, %f, %f) \n\t(%f, %f, %f) \n", x.x, x.y, x.z, dx_x.x, dx_x.y, dx_x.z);

	/*************************************************************************/
	printf("Testing vector cross\n");
	z = cross(x, y);
	D3DXVec3Cross(&dx_z, &dx_x, &dx_y);
	printf("Cross( \n\t(%f, %f, %f), \n\t(%f, %f, %f) \n\t) = (%f, %f, %f)\n", x.x, x.y, x.z, y.x, y.y, y.z, z.x, z.y, z.z);
	printf("Dx Cross( \n\t(%f, %f, %f), \n\t(%f, %f, %f) \n\t) = (%f, %f, %f)\n", dx_x.x, dx_x.y, dx_x.z, dx_y.x, dx_y.y, dx_y.z, dx_z.x, dx_z.y, dx_z.z);

	/*************************************************************************/
	printf("Testing matrix inversion\n");
	matrix4x4 m4x4, inv_m4x4;
	D3DXMATRIX dx_m4x4, dx_inv_m4x4;

	for (int ir = 0; ir < 4; ir++)
	{
			dx_m4x4.m[ir][0] = m4x4.row[ir].x = (rand() % 1025) / 1024.0f;
			dx_m4x4.m[ir][1] = m4x4.row[ir].y = (rand() % 1025) / 1024.0f;
			dx_m4x4.m[ir][2] = m4x4.row[ir].z = (rand() % 1025) / 1024.0f;
			dx_m4x4.m[ir][3] = m4x4.row[ir].w = (rand() % 1025) / 1024.0f;
	}

	float determinant, dx_determinant;

	determinant = Matrix4x4Determinant(&m4x4);
	dx_determinant = D3DXMatrixDeterminant(&dx_m4x4);

	Matrix4x4Inverse(&inv_m4x4, &m4x4);
	D3DXMatrixInverse(&dx_inv_m4x4, &dx_determinant, &dx_m4x4);

	float matrix_error = 0.0f, inv_matrix_error = 0.0f;

	for (int ir = 0; ir < 4; ir++)
	{
			matrix_error += abs(dx_m4x4.m[ir][0] - m4x4.row[ir].x) +
											abs(dx_m4x4.m[ir][1] - m4x4.row[ir].y) +
											abs(dx_m4x4.m[ir][2] - m4x4.row[ir].z) +
											abs(dx_m4x4.m[ir][3] - m4x4.row[ir].w);
	}

	for (int ir = 0; ir < 4; ir++)
	{
			inv_matrix_error += abs(dx_inv_m4x4.m[ir][0] - inv_m4x4.row[ir].x) +
													abs(dx_inv_m4x4.m[ir][1] - inv_m4x4.row[ir].y) +
													abs(dx_inv_m4x4.m[ir][2] - inv_m4x4.row[ir].z) +
													abs(dx_inv_m4x4.m[ir][3] - inv_m4x4.row[ir].w);
	}

	printf("Determinant = %f \n", determinant);
	printf("Dx Determinant = %f \n", dx_determinant);
	printf("Matrix abs sum error = %f \n", matrix_error);
	printf("Inv Matrix abs sum error = %f \n", inv_matrix_error);

	/*************************************************************************/
	printf("Testing matrix operation * \n");
	m4x4 *= 5.0f;
	dx_m4x4 *= 5.0f;

	matrix_error = 0.0f;

	for (int ir = 0; ir < 4; ir++)
	{
			matrix_error += abs(dx_m4x4.m[ir][0] - m4x4.row[ir].x) +
											abs(dx_m4x4.m[ir][1] - m4x4.row[ir].y) +
											abs(dx_m4x4.m[ir][2] - m4x4.row[ir].z) +
											abs(dx_m4x4.m[ir][3] - m4x4.row[ir].w);
	}
	printf("Matrix abs sum error = %f \n", matrix_error);

	/*************************************************************************/
	printf("Testing matrix operation / \n");
	m4x4 /= 5.0f;
	dx_m4x4 /= 5.0f;

	matrix_error = 0.0f;

	for (int ir = 0; ir < 4; ir++)
	{
			matrix_error += abs(dx_m4x4.m[ir][0] - m4x4.row[ir].x) +
											abs(dx_m4x4.m[ir][1] - m4x4.row[ir].y) +
											abs(dx_m4x4.m[ir][2] - m4x4.row[ir].z) +
											abs(dx_m4x4.m[ir][3] - m4x4.row[ir].w);
	}
	printf("Matrix abs sum error = %f \n", matrix_error);

	/*************************************************************************/
	printf("Test PerspectiveFovLH \n");
	const float pi = 3.1415f;
	Matrix4x4PerspectiveFovLH(&m4x4, pi / 4.0f, 1600.0f / 1200.0f, 0.1f, 1000.0f);
	D3DXMatrixPerspectiveFovLH(&dx_m4x4, pi / 4.0f, 1600.0f / 1200.0f, 0.1f, 1000.0f);

	matrix_error = 0.0f;

	for (int ir = 0; ir < 4; ir++)
	{
			matrix_error += abs(dx_m4x4.m[ir][0] - m4x4.row[ir].x) +
											abs(dx_m4x4.m[ir][1] - m4x4.row[ir].y) +
											abs(dx_m4x4.m[ir][2] - m4x4.row[ir].z) +
											abs(dx_m4x4.m[ir][3] - m4x4.row[ir].w);
	}
	printf("Matrix abs sum error = %f \n", matrix_error);

	/*************************************************************************/
	printf("Test Translate \n");
	Matrix4x4TranslateR(&m4x4, x);
	D3DXMatrixTranslation(&dx_m4x4, dx_x.x, dx_x.y, dx_x.z);

	matrix_error = 0.0f;

	for (int ir = 0; ir < 4; ir++)
	{
			matrix_error += abs(dx_m4x4.m[ir][0] - m4x4.row[ir].x) +
											abs(dx_m4x4.m[ir][1] - m4x4.row[ir].y) +
											abs(dx_m4x4.m[ir][2] - m4x4.row[ir].z) +
											abs(dx_m4x4.m[ir][3] - m4x4.row[ir].w);
	}
	printf("Matrix abs sum error = %f \n", matrix_error);

	/*************************************************************************/
	printf("Test Rotate \n");
	float3 ijk = {1, 1, 1};
	D3DXVECTOR3 dx_ijk (1, 1, 1);
	float alpha = 60.0f;
	Matrix4x4RotateAxisR(&m4x4, ijk, M_PI / 3.0f);
	D3DXMatrixRotationAxis(&dx_m4x4, (D3DXVECTOR3 *) &dx_ijk, M_PI / 3.0f);

	matrix_error = 0.0f;

	for (int ir = 0; ir < 4; ir++)
	{
			matrix_error += abs(dx_m4x4.m[ir][0] - m4x4.row[ir].x) +
											abs(dx_m4x4.m[ir][1] - m4x4.row[ir].y) +
											abs(dx_m4x4.m[ir][2] - m4x4.row[ir].z) +
											abs(dx_m4x4.m[ir][3] - m4x4.row[ir].w);
	}
	printf("Matrix abs sum error = %f \n", matrix_error);

	/*************************************************************************/
	printf("Test Rotate YPR\n");
	matrix4x4 ml4x4, ml4x4_T;
	Matrix4x4RotateYawPithRollR(&m4x4, M_PI / 2.0f, M_PI / 3.0f, M_PI / 6.0f);
	Matrix4x4RotateYawPithRollL(&ml4x4, M_PI / 2.0f, M_PI / 3.0f, M_PI / 6.0f);
	Matrix4x4Transpose(&ml4x4_T, &ml4x4);
	D3DXMatrixRotationYawPitchRoll(&dx_m4x4, M_PI / 2.0f, M_PI / 3.0f, M_PI / 6.0f);

	matrix_error = 0.0f;

	for (int ir = 0; ir < 4; ir++)
	{
			matrix_error += abs(dx_m4x4.m[ir][0] - m4x4.row[ir].x) +
											abs(dx_m4x4.m[ir][1] - m4x4.row[ir].y) +
											abs(dx_m4x4.m[ir][2] - m4x4.row[ir].z) +
											abs(dx_m4x4.m[ir][3] - m4x4.row[ir].w);
	}
	printf("Matrix abs sum error = %f \n", matrix_error);

	for (int ir = 0; ir < 4; ir++)
	{
			matrix_error += abs(ml4x4_T.row[ir].x - m4x4.row[ir].x) +
											abs(ml4x4_T.row[ir].y - m4x4.row[ir].y) +
											abs(ml4x4_T.row[ir].z - m4x4.row[ir].z) +
											abs(ml4x4_T.row[ir].w - m4x4.row[ir].w);
	}
	printf("Matrix abs sum error = %f \n", matrix_error);

	/*************************************************************************/
	printf("Test LookAtLH \n");
	Matrix4x4LookAtLHR(&m4x4, x, x+z, y);
	D3DXVECTOR3 At = (dx_x+dx_z);
	D3DXMatrixLookAtLH(&dx_m4x4, &dx_x, &At, &dx_y);

	matrix_error = 0.0f;

	for (int ir = 0; ir < 4; ir++)
	{
			matrix_error += abs(dx_m4x4.m[ir][0] - m4x4.row[ir].x) +
											abs(dx_m4x4.m[ir][1] - m4x4.row[ir].y) +
											abs(dx_m4x4.m[ir][2] - m4x4.row[ir].z) +
											abs(dx_m4x4.m[ir][3] - m4x4.row[ir].w);
	}
	printf("Matrix abs sum error = %f \n", matrix_error);

	/*************************************************************************/
	printf("Test LookAtRH \n");
	Matrix4x4LookAtRHR(&m4x4, x, x+z, y);
	At = (dx_x+dx_z);
	D3DXMatrixLookAtRH(&dx_m4x4, &dx_x, &At, &dx_y);

	matrix_error = 0.0f;

	for (int ir = 0; ir < 4; ir++)
	{
			matrix_error += abs(dx_m4x4.m[ir][0] - m4x4.row[ir].x) +
											abs(dx_m4x4.m[ir][1] - m4x4.row[ir].y) +
											abs(dx_m4x4.m[ir][2] - m4x4.row[ir].z) +
											abs(dx_m4x4.m[ir][3] - m4x4.row[ir].w);
	}
	printf("Matrix abs sum error = %f \n", matrix_error);

	/*************************************************************************/
	printf("Test Transpose \n");
	matrix4x4 trans_m4x4;
	D3DXMATRIX dx_trans_m4x4;
	Matrix4x4Transpose(&trans_m4x4, &m4x4);    
	D3DXMatrixTranspose(&dx_trans_m4x4, &dx_m4x4);

	matrix_error = 0.0f;

	for (int ir = 0; ir < 4; ir++)
	{
			matrix_error += abs(dx_trans_m4x4.m[ir][0] - trans_m4x4.row[ir].x) +
											abs(dx_trans_m4x4.m[ir][1] - trans_m4x4.row[ir].y) +
											abs(dx_trans_m4x4.m[ir][2] - trans_m4x4.row[ir].z) +
											abs(dx_trans_m4x4.m[ir][3] - trans_m4x4.row[ir].w);
	}
	printf("Matrix abs sum error = %f \n", matrix_error);

	/*************************************************************************/
	printf("\nTest with GL functions Need To create ogl window for this\n");
	glutInit( &argc, argv );    
	glutInitWindowSize(100, 100);    
	glutInitWindowPosition(0,0);
	glutInitDisplayMode(GLUT_RGBA);

	int winid = glutCreateWindow("glutCommon window");

	printf("Test Rotate \n");
	matrix4x4 rotate4x4;
	Matrix4x4RotateAxisR(&rotate4x4, ijk, M_PI / 3.0f);    
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
	glRotatef(alpha, ijk.x, ijk.y, ijk.z);
	float gl_rotate[4][4];
	glGetFloatv(GL_MODELVIEW_MATRIX, (GLfloat *) gl_rotate);

	matrix_error = 0.0f;

	for (int ir = 0; ir < 4; ir++)
	{
			matrix_error += abs(gl_rotate[ir][0] - rotate4x4.row[ir].x) +
											abs(gl_rotate[ir][1] - rotate4x4.row[ir].y) +
											abs(gl_rotate[ir][2] - rotate4x4.row[ir].z) +
											abs(gl_rotate[ir][3] - rotate4x4.row[ir].w);
	}
	printf("Matrix abs sum error = %f \n", matrix_error);

	printf("Test Translate \n");
	matrix4x4 translate4x4;
	Matrix4x4TranslateR(&translate4x4, ijk);    
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
	glTranslatef(ijk.x, ijk.y, ijk.z);
	float4 gl_translate[4];
	glGetFloatv(GL_MODELVIEW_MATRIX, (GLfloat *) gl_translate);

	matrix_error = 0.0f;

	for (int ir = 0; ir < 4; ir++)
	{
			matrix_error += abs(gl_translate[ir].x - translate4x4.row[ir].x) +
											abs(gl_translate[ir].y - translate4x4.row[ir].y) +
											abs(gl_translate[ir].z - translate4x4.row[ir].z) +
											abs(gl_translate[ir].w - translate4x4.row[ir].w);
	}
	printf("Matrix abs sum error = %f \n", matrix_error);

	printf("Test LookAt\n");
	matrix4x4 lookat4x4;
	Matrix4x4LookAtRHR(&lookat4x4, x, x+z, y);    
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
	float3 center = x+z;
	gluLookAt(x.x, x.y, x.z, center.x, center.y, center.z, y.x, y.y, y.z);
	float4 gl_lookat[4];
	glGetFloatv(GL_MODELVIEW_MATRIX, (GLfloat *) gl_lookat);

	matrix_error = 0.0f;

	for (int ir = 0; ir < 4; ir++)
	{
			matrix_error += abs(gl_lookat[ir].x - lookat4x4.row[ir].x) +
											abs(gl_lookat[ir].y - lookat4x4.row[ir].y) +
											abs(gl_lookat[ir].z - lookat4x4.row[ir].z) +
											abs(gl_lookat[ir].w - lookat4x4.row[ir].w);
	}
	printf("Matrix abs sum error = %f \n", matrix_error);

	printf("Test Perspective\n");
	matrix4x4 perspective4x4, perspective4x4_T;
	Matrix4x4PerspectiveFovGLL(&perspective4x4, M_PI / 3.0f, 1.0f, 0.1f, 1000.0f);
	Matrix4x4Transpose(&perspective4x4_T, &perspective4x4);
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	gluPerspective(60.0f, 1.0f, 0.1f, 1000.0f);
	float4 gl_perspective[4];
	glGetFloatv(GL_PROJECTION_MATRIX, (GLfloat *) gl_perspective);

	matrix_error = 0.0f;

	for (int ir = 0; ir < 4; ir++)
	{
			matrix_error += abs(gl_perspective[ir].x - perspective4x4_T.row[ir].x) +
											abs(gl_perspective[ir].y - perspective4x4_T.row[ir].y) +
											abs(gl_perspective[ir].z - perspective4x4_T.row[ir].z) +
											abs(gl_perspective[ir].w - perspective4x4_T.row[ir].w);
	}
	printf("Matrix abs sum error = %f \n", matrix_error);

	glutDestroyWindow(winid);

	rnInit();

	TBuffer<KeyValuePair> buffer;
	TBuffer<float> fbuffer;
	TBuffer<float> fbuffer2;

	int n = 128;

	int_sorter.Create(n, BUFFER_H);
	float_sorter.Create(n, BUFFER_H);
	kvp_sorter.Create(n, BUFFER_H);

	buffer.Create(n, BUFFER_H);
	fbuffer.Create(n, BUFFER_H);
	fbuffer2.Create(n, BUFFER_H);

	KeyValuePair * p = buffer.hPtrR();
	float * pf = fbuffer.hPtrR();
	float * pf2 = fbuffer2.hPtrR();

	memset(p, 0, sizeof(KeyValuePair) * n);

	for (int32 i = 0; i < n; i++)
	{
		p[i].x = rnsFloat(0xFFFF);
		pf[i] = p[i].x;
		pf2[i] = p[i].x;
	}

	sorter.Sort(pf, n); // for fare comparison - allocate resources now

	for (int32 i = 0; i < n; i++)
	{
		pf[i] = p[i].x;
	}

	//for (int i = 0; i < n; i++)
	//	printf("%f\n", p[i].key);

	printf("-------------------------------\n");

	//////////////////////////////////////////////////////////////////////////
	/// Keyed Radix sort
	//////////////////////////////////////////////////////////////////////////
	clock_t start = clock();
	kvp_sorter.SortKey( /*(TBuffer<uint32> *)*/ &buffer);
	clock_t end = clock();

	printf("My Radix Key, first sort: end - start = %d ms\n", end - start);

	start = clock();
	kvp_sorter.SortKey( /*(TBuffer<uint32> *)*/ &buffer);
	end = clock();

	printf("My Radix Key, second sort: end - start = %d ms\n", end - start);

	//////////////////////////////////////////////////////////////////////////
	/// Float Radix sort
	//////////////////////////////////////////////////////////////////////////

	start = clock();
	float_sorter.Sort( /*(TBuffer<uint32> *)*/ &fbuffer2, BUFFER_RT, BUFFER_RT);
	end = clock();

	printf("My Radix float, first sort: end - start = %d ms\n", end - start);

	start = clock();
	float_sorter.Sort( /*(TBuffer<uint32> *)*/ &fbuffer2);
	end = clock();

	printf("My Radix float, second sort: end - start = %d ms\n", end - start);

	//////////////////////////////////////////////////////////////////////////
	/// IceRevisited Radix Float sort
	//////////////////////////////////////////////////////////////////////////

	start = clock();
	sorter.Sort(pf, n);
	end = clock();

	printf("IceRevisitedRadix, first sort: end - start = %d ms\n", end - start);

	start = clock();
	sorter.Sort(pf, n);
	end = clock();

	printf("IceRevisitedRadix, second sort: end - start = %d ms\n", end - start);

	//////////////////////////////////////////////////////////////////////////
	/// Error checking
	//////////////////////////////////////////////////////////////////////////

	printf("Check for errors...\n");
	uint32 * pii = sorter.GetIndices();
	for (int i = 0; i < n; i++)
	{
		if (p[i].x != pf[pii[i]] || pf2[i] != pf[pii[i]])
			printf("%f, %f, %f %d\n", p[i].x, pf[pii[i]], pf2[i], i);
	}
	printf(".. done!\n:");

	TBuffer<int32> i32buffer;

	i32buffer.Create(n, BUFFER_H, false);

	int * ip = (int *) i32buffer.hPtrR();

	for (int i = 0; i < n; i++)
		ip[i] = rnsFloat(0xFFFF) * 128;

	int_sorter.Sort( &i32buffer, true, (uint32) 0, (uint32) 0);

	for (int i = 0; i < n; i++)
		printf("%d\n", ip[i]);

	histopyramid.Create(4, BUFFER_H, false);

	histopyramid.Histogram( (sf1Buffer *) &fbuffer2, &f2(pf2[0], pf2[n-1]), NULL, EXEC_H, BUFFER_RT);

	histopyramid.Pyramid(EXEC_H, BUFFER_RT);

	histopyramid.Scan(EXEC_H, BUFFER_RT);

	return 0;
}